void InterpretSerial()
{
  
  //1. count lenght of InputBuffer (Status Request or Command)
  // Status Request: 2 Bytes : Byte 1, Startbyte 0x0F + Byte 2, LF Byte 0x0A
  // Command: 26 Bytes
  
  if (inputBuffer[0]==0x0F && inputBuffer[1]==0xFF) // Status Request
  {
            //Serial.print(inputBuffer);  
            Serialstatus(); //send Status
            EmptyInputBuffer();
            return;
  }
  
  if (inputBuffer[0]==0x0F && inputBuffer[25]==0x0A) // Command
  {
            SerialCommand();
            Serial.print("OK\n");  
            EmptyInputBuffer();
            return;
  } 
  
  if (inputBuffer[0]==0x70 && inputBuffer[1]==0x69 && inputBuffer[2]==0x6E && inputBuffer[3]==0x67) // ping
   {
     //Serial.print("PONG\n");  
     //Serial.write(inputBuffer);
     Serialstatus(); //send Status
     EmptyInputBuffer();
     return;
   }
  
   /*
   if (inputBuffer[0]==0x70 && inputBuffer[1]==0x69 && inputBuffer[2]==0x6E && inputBuffer[3]==0x67) // ping
   {
     Serial.print("PONG\n");  
     EmptyInputBuffer();
     return;
   }
   else
     {
     if (inputBuffer[0]==0x0F && inputBuffer[1]==0x0A && inputBuffer[25]==0) // Status Request
     {
            Serialstatus(); //send Status
            EmptyInputBuffer();
            return;
     }
     else
     {
        if (inputBuffer[0]==0x0F && inputBuffer[25]==0x0A) // Command
        {
            SerialCommand();  
            EmptyInputBuffer();
            return;
        } 
     }
    }
    */
    //Unknown Frame
    Serial.print("UNKOWN COMMAND\n");  
    //Serial.write(inputBuffer);
    lmspeed=0;                                               // bad data received
    rmspeed=0;                                               // set motor speeds to 0 
    EmptyInputBuffer();
    return;
    
}

void SerialCommand()
{
  
 
int i;
byte b;

  b=inputBuffer[1];                                                             // read pwmfreq from the buffer
  if(b>0 && b<8)                                                               // if value is valid (1-7)  
  {
    pwmfreq=b;                                                                 // update pwmfreq
    TCCR2B = TCCR2B & B11111000 | pwmfreq;                                     // change timer 2 clock pre-scaler
  }
  else
  {
    errorflag = errorflag | 2;                                                 // incorrect pwmfreq given
  }
  
    i= (inputBuffer[2]<<8)+inputBuffer[3]; //HighByte + LowByte
    lmspeed=i;
    
    b=inputBuffer[4];
    lmbrake=b;    
    
    i= (inputBuffer[5]<<8)+inputBuffer[6]; //HighByte + LowByte
    rmspeed=i;
    
    b=inputBuffer[7];
    rmbrake=b;    
    
    i= (inputBuffer[8]<<8)+inputBuffer[9]; //HighByte + LowByte
    if(abs(i)>2400) errorflag = errorflag | 8;                                 // incorrect servo position given
    servopos[0]=i;                                                             // read new servo position -- 0 = no servo present
    
    i= (inputBuffer[10]<<8)+inputBuffer[11]; //HighByte + LowByte
    if(abs(i)>2400) errorflag = errorflag | 8;                                 // incorrect servo position given
    servopos[1]=i;                                                             // read new servo position -- 0 = no servo present
    
    i= (inputBuffer[12]<<8)+inputBuffer[13]; //HighByte + LowByte
    if(abs(i)>2400) errorflag = errorflag | 8;                                 // incorrect servo position given
    servopos[2]=i;                                                             // read new servo position -- 0 = no servo present
    
    i= (inputBuffer[14]<<8)+inputBuffer[15]; //HighByte + LowByte
    if(abs(i)>2400) errorflag = errorflag | 8;                                 // incorrect servo position given
    servopos[3]=i;                                                             // read new servo position -- 0 = no servo present
    
    i= (inputBuffer[16]<<8)+inputBuffer[17]; //HighByte + LowByte
    if(abs(i)>2400) errorflag = errorflag | 8;                                 // incorrect servo position given
    servopos[4]=i;                                                             // read new servo position -- 0 = no servo present
    
    i= (inputBuffer[18]<<8)+inputBuffer[19]; //HighByte + LowByte
    if(abs(i)>2400) errorflag = errorflag | 8;                                 // incorrect servo position given
    servopos[5]=i;                                                             // read new servo position -- 0 = no servo present
    
    b=inputBuffer[20];
    devibrate=b;    
    
    i= (inputBuffer[21]<<8)+inputBuffer[22]; //HighByte + LowByte
    if(i>-1 && i<1024){
    sensitivity=i;                                                             // impact sensitivity from 0-1023 - default is 50
  }
  else {
    errorflag = errorflag | 16;                                                // incorrect sensitivity given
  }
  
   i= (inputBuffer[23]<<8)+inputBuffer[24]; //HighByte + LowByte
  if(i>549 && i<3001)
  {
    lowbat=i;                                                                  // set low battery value (values higher than battery voltage will force a shutdown)
  }
  else
  {
    errorflag = errorflag | 32;                                                // incorrect lowbat given
  }
  
  
  mode=5;                                                                      // breaks out of Shutdown mode when I²C command is given
  Motors();                                                                    // update brake, speed and direction of motors
  Servos();                                                                    // update servo positions
  
  }
  
  /*
  ===== Command data packet - 26 bytes =====
byte  Start 0x0F
byte  PWMfreq 1=31.25kHz    7=30.5Hz
int   lmspeed
byte  lmbrake
int   rmspeed
byte  rmbrake
int   servo 0
int   servo 1
int   servo 2
int   servo 3
int   servo 4
int   servo 5
byte  devibrate                       default=50 (100mS)   0-255
int   impact sensitivity  0 to 1023   default=50  
int   low battery  550 to 30000       5.5V to 30V
byte EndByte 0x0A



  */



void EmptyInputBuffer()
{
  for (int i=0;i++; i<sizeof(inputBuffer))
 {
   inputBuffer[i]=0x00;
 }
 
}



void SerialConfig()                                         // This code intended for a DAGU bluetooth module - may not work with other brands
{
  //long baud[]={9600,115200,57600,38400,19200,4800,2400,1200};  // try 9600 first as this is default setting then try other baud rates
  //byte br=0,d;
  //while(mode==0 && br<8)                                       // scan through different baud rates and attempt to configure bluetooth module
  
  Serial.begin(115200);                                    // enable T'REX serial at baud rate baud[br]
  Serial.print("TREX");                                        // send "AT" to see if bluetooth module is connected
  
  delay(1500);                                               // wait for bluetooth module to respond
    // reserve 200 bytes for the inputString:
  //inputString.reserve(200);
 
  mode=5;
//  EmptyBuffer();
  
  
}


/*
   SerialEvent occurs whenever a new data comes in the
  hardware serial RX.  This routine is run between each
  time loop() runs, so using delay inside loop can delay
  response.  Multiple bytes of data may be available.
  */
void serialEvent() {
   while (Serial.available()) {
      
     // get the new byte:
     char inChar = (char)Serial.read(); 
          // add it to the inputString:
          // add it into Bytearray
          inputBuffer[BufferIndex]=inChar;
          BufferIndex++;
          Serial.print(inChar);
                    
//     inputString += inChar;
     // if the incoming character is a newline, set a flag
     // so the main loop can do something about it:
     if (inChar == 0x0A) { //LF '\n'
       BufferIndex =0;
       SerialCommandComplete = true;
     } 
   }
}


